﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using System.IO;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Kinect.Toolkit.FaceTracking;

namespace Brain.Kinect
{
    public class KinectManager : IEngineManager, IDisposable, IUpdateable, IDrawable
    {
        /// <summary>
        /// The Selected Kinect Sensor
        /// </summary>
        public KinectSensor Sensor;

        /// <summary>
        /// The Last Know status of the Sensor
        /// </summary>
        public KinectStatus LastStatus;

        /// <summary>
        /// The Requested Color Image Format
        /// </summary>
        public ColorImageFormat ColorImageFormat { get; protected set; }

        /// <summary>
        /// The Requested Depth Image Format
        /// </summary>
        public DepthImageFormat DepthImageFormat { get; protected set; }

        public bool ShouldUpdate { get; set; }
        public bool ShouldDraw { get; set; }

        public bool ReadSkeleton { get; set; }
        protected Skeleton[] SkeletonData;

        public bool ReadFaceTracking { get; set; }
        protected FaceTracker FaceTracker;
        protected bool lastFaceTrackSucceeded = false;
        protected FaceTriangle[] faceTriangles;
        protected EnumIndexableCollection<FeaturePoint, PointF> facePoints;

        protected Texture2D ColorStreamTexture;
        protected byte[] ColorStreamData;
        protected Texture2D DepthStreamTexture;
        protected short[] DepthStreamData;

        public KinectManager(ColorImageFormat color, DepthImageFormat depth)
        {
            this.ColorImageFormat = color;
            this.DepthImageFormat = depth;

            ShouldUpdate = true;
            ShouldDraw = true;

            ReadSkeleton = true;

            KinectSensor.KinectSensors.StatusChanged += new EventHandler<StatusChangedEventArgs>(KinectSensors_StatusChanged);
            DiscoverSensor();
        }

        public Skeleton[] GetSkeletonData()
        {
            return this.SkeletonData;
        }
        public Texture2D GetColorTexture()
        {
            if (ColorStreamTexture == null ||
                ColorStreamTexture.IsDisposed)
            {
                return null;
            }
            return ColorStreamTexture;
        }
        public Texture2D GetDepthTexture()
        {
            if (DepthStreamTexture == null ||
                DepthStreamTexture.IsDisposed)
            {
                return null;
            }
            return DepthStreamTexture;
        }

        public void Update()
        {
            if (this.Sensor == null ||
                !Sensor.IsRunning ||
                this.Sensor.Status != KinectStatus.Connected)
            {
                return;
            }

            if (Drawed)
            {
                if (this.ColorImageFormat != ColorImageFormat.Undefined)
                {
                    using (ColorImageFrame frame = this.Sensor.ColorStream.OpenNextFrame(0))
                    {
                        // Sometimes we get a null frame back if no data is ready
                        if (frame == null)
                        {
                            return;
                        }

                        // Reallocate values if necessary
                        if (this.ColorStreamData == null || this.ColorStreamData.Length != frame.PixelDataLength)
                        {
                            this.ColorStreamData = new byte[frame.PixelDataLength];

                            this.ColorStreamTexture = new Texture2D(
                                Engine.Instance.GraphicsDevice,
                                frame.Width,
                                frame.Height,
                                false,
                                SurfaceFormat.Color);
                        }

                        frame.CopyPixelDataTo(this.ColorStreamData);
                        this.ColorStreamTexture.SetData(this.ColorStreamData);
                    }
                }

                if (this.DepthImageFormat != DepthImageFormat.Undefined)
                {
                    using (DepthImageFrame frame = this.Sensor.DepthStream.OpenNextFrame(0))
                    {
                        // Sometimes we get a null frame back if no data is ready
                        if (frame == null)
                        {
                            return;
                        }

                        // Reallocate values if necessary
                        if (this.DepthStreamData == null || this.DepthStreamData.Length != frame.PixelDataLength)
                        {
                            this.DepthStreamData = new short[frame.PixelDataLength];

                            this.DepthStreamTexture = new Texture2D(
                                Engine.Instance.GraphicsDevice,
                                frame.Width,
                                frame.Height,
                                false,
                                SurfaceFormat.Bgra4444);
                        }

                        frame.CopyPixelDataTo(this.DepthStreamData);
                        this.DepthStreamTexture.SetData(this.DepthStreamData);
                    }
                }

                if (ReadSkeleton || ReadFaceTracking)
                {
                    using (SkeletonFrame frame = this.Sensor.SkeletonStream.OpenNextFrame(0))
                    {
                        if (frame == null)
                        {
                            return;
                        }

                        if (SkeletonData == null ||
                            SkeletonData.Length != frame.SkeletonArrayLength)
                        {
                            SkeletonData = new Skeleton[frame.SkeletonArrayLength];
                        }

                        if (FaceTracker == null)
                        {
                            try
                            {
                                this.FaceTracker = new FaceTracker(Sensor);
                            }
                            catch (InvalidOperationException)
                            {
                                // During some shutdown scenarios the FaceTracker
                                // is unable to be instantiated.  Catch that exception
                                // and don't track a face.
                                this.FaceTracker = null;
                            }
                        }

                        frame.CopySkeletonDataTo(SkeletonData);

                        if (this.FaceTracker != null)
                        {
                            FaceTrackFrame faceFrame = this.FaceTracker.Track(
                                this.ColorImageFormat, this.ColorStreamData, this.DepthImageFormat, this.DepthStreamData);

                            this.lastFaceTrackSucceeded = faceFrame.TrackSuccessful;
                            if (this.lastFaceTrackSucceeded)
                            {
                                if (faceTriangles == null)
                                {
                                    // only need to get this once.  It doesn't change.
                                    faceTriangles = faceFrame.GetTriangles();
                                }

                                this.facePoints = faceFrame.GetProjected3DShape();
                            }
                        }
                    }
                }

                Drawed = false;
            }
        }


        private bool Drawed = false;

        public void Draw()
        {
            Drawed = true;
        }

        public void Dispose()
        {
            // Always stop the sensor when closing down
            if (this.Sensor != null)
            {
                this.Sensor.Stop();
            }

            if (this.ColorStreamTexture != null &&
                !this.ColorStreamTexture.IsDisposed)
            {
                this.ColorStreamTexture.Dispose();
            }
            if (this.DepthStreamTexture != null &&
                !this.DepthStreamTexture.IsDisposed)
            {
                this.DepthStreamTexture.Dispose();
            }

            ColorStreamTexture = null;
            ColorStreamData = null;
            DepthStreamData = null;
            DepthStreamTexture = null;
            
            if (this.FaceTracker != null)
            {
                this.FaceTracker.Dispose();
                this.FaceTracker = null;
            }
        }

        /// <summary>
        /// This wires up the status changed event to monitor for 
        /// Kinect state changes.  It automatically stops the sensor
        /// if the device is no longer available.
        /// </summary>
        /// <param name="sender">The sending object.</param>
        /// <param name="e">The event args.</param>
        private void KinectSensors_StatusChanged(object sender, StatusChangedEventArgs e)
        {
            // If the status is not connected, try to stop it
            if (e.Status != KinectStatus.Connected)
            {
                e.Sensor.Stop();
            }

            this.LastStatus = e.Status;
            this.DiscoverSensor();
        }
        /// <summary>
        /// This method will use basic logic to try to grab a sensor.
        /// Once a sensor is found, it will start the sensor with the
        /// requested options.
        /// </summary>
        protected void DiscoverSensor()
        {
            // Grab any available sensor
            this.Sensor = KinectSensor.KinectSensors.FirstOrDefault();

            if (this.Sensor != null)
            {
                this.LastStatus = this.Sensor.Status;

                // If this sensor is connected, then enable it
                if (this.LastStatus == KinectStatus.Connected)
                {
                    this.Sensor.SkeletonStream.Enable();
                    if (this.ColorImageFormat != ColorImageFormat.Undefined)
                    {
                        this.Sensor.ColorStream.Enable(this.ColorImageFormat);
                    }
                    if (DepthImageFormat != DepthImageFormat.Undefined)
                    {
                        this.Sensor.DepthStream.Enable(this.DepthImageFormat);
                    }

                    try
                    {
                        this.Sensor.Start();
                    }
                    catch (IOException)
                    {
                        // sensor is in use by another application
                        // will treat as disconnected for display purposes
                        this.Sensor = null;
                    }
                }
            }
            else
            {
                this.LastStatus = KinectStatus.Disconnected;
            }
        }

        public void Reset()
        {
        }

        public SerializationData Data { get; set; }
        public void SerializeData(DefaultSerializationManager manager)
        {
        }
        public void DeserializeData(DefaultSerializationManager manager, SerializationData newData)
        {
        }
    }
}
